﻿#include <boost/thread/thread.hpp>
#include <pcl/common/transforms.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/random_sample.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/surface/gp3.h>//贪婪投影三角化算法类定义的头文件
#include <pcl/visualization/pcl_visualizer.h>
#include <random>

//fpfh算法通过法线计算点云特征
pcl::PointCloud<pcl::FPFHSignature33>::Ptr compute_fpfh(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	//计算法线
	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
	ne.setInputCloud(cloud);
	ne.setSearchMethod(tree);
	ne.setKSearch(10);
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	ne.compute(*normals);
	//计算FPFH特征
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh(new pcl::PointCloud<pcl::FPFHSignature33>);
	pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
	fpfh_est.setInputCloud(cloud);
	fpfh_est.setInputNormals(normals);
	fpfh_est.setSearchMethod(tree);
	fpfh_est.setKSearch(10);
	fpfh_est.compute(*fpfh);
	return fpfh;
}

int
main()
{
	//载入并初始化点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_processed(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPLYFile("skull_source.ply", *cloud_in) < 0)
	{
		PCL_ERROR("Error loading cloud skull_source.ply.\n");
		return (-1);
	}
	std::cout << "PointCloud before filtering: " << cloud_in->width * cloud_in->height
		<< " data points (" << pcl::getFieldsList(*cloud_in) << ")." << endl;
	int num_points = cloud_in->points.size();

	// 随机采样点云
	float keep_ratio = 0.9f; // 设定保留比例
	int sample_num = static_cast<int>(num_points * keep_ratio);
	pcl::RandomSample<pcl::PointXYZ> random_sampler;
	random_sampler.setInputCloud(cloud_in);
	random_sampler.setSample(sample_num);
	random_sampler.filter(*cloud_processed);
	std::cout << "PointCloud after filtering: " << cloud_processed->width * cloud_processed->height
		<< " data points (" << pcl::getFieldsList(*cloud_processed) << ")." << endl;

	std::random_device rd; // 获取随机数种子
	std::mt19937 gen(rd());

	//随机平移点云
	std::uniform_real_distribution<float> dis1(1.0, 10.0); // 1~10随机数
	float x_rand = dis1(gen);
	float y_rand = dis1(gen);
	float z_rand = dis1(gen);
	for (auto& point : *cloud_processed) {
		point.x = point.x + x_rand;
		point.y = point.y + y_rand;
		point.z = point.z + z_rand;
	}
	std::cout << "平移幅度: (" << x_rand << ", " << y_rand << ", " << z_rand << ")" << std::endl;

	// 随机旋转点云
	std::uniform_real_distribution<float> dis2(0.0, 2 * M_PI); // 0~2π随机角度
	float angle_x = dis2(gen);
	float angle_y = dis2(gen);
	float angle_z = dis2(gen);
	Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
	transform.block<3, 3>(0, 0) =
		Eigen::AngleAxisf(angle_z, Eigen::Vector3f::UnitZ()) *
		Eigen::AngleAxisf(angle_y, Eigen::Vector3f::UnitY()) *
		Eigen::AngleAxisf(angle_x, Eigen::Vector3f::UnitX()).toRotationMatrix();
	pcl::transformPointCloud(*cloud_processed, *cloud_processed, transform);
	std::cout << "旋转角度: (" << angle_x << ", " << angle_y << ", " << angle_z << ")" << std::endl;

	//compute_fpfh计算点云特征
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_in = compute_fpfh(cloud_in);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_processed = compute_fpfh(cloud_processed);

	// ransac进行点云粗配准
	pcl::SampleConsensusPrerejective<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sample_consensus;
	sample_consensus.setInputSource(cloud_in);
	sample_consensus.setInputTarget(cloud_processed);
	sample_consensus.setSourceFeatures(fpfh_in);
	sample_consensus.setTargetFeatures(fpfh_processed);
	sample_consensus.setCorrespondenceRandomness(5);
	sample_consensus.setInlierFraction(0.5f);
	sample_consensus.setNumberOfSamples(3);
	sample_consensus.setSimilarityThreshold(0.1f);
	sample_consensus.setMaxCorrespondenceDistance(1.0f);
	sample_consensus.setMaximumIterations(100);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_aligned(new pcl::PointCloud<pcl::PointXYZ>);
	sample_consensus.align(*cloud_aligned);
	std::cout << "RANSAC has" << (sample_consensus.hasConverged() ? " converged." : " not converged.") <<"score is:"<<sample_consensus.getFitnessScore() << std::endl;
	Eigen::Matrix4f final_transform_sac = sample_consensus.getFinalTransformation();
	std::cout << "RANSAC Final transformation: " << final_transform_sac << std::endl;

	//icp进行点云精配准
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(cloud_aligned);
	/*if (cloud_aligned->empty() || cloud_processed->empty()) {
		std::cerr << "cloud_aligned or cloud_processed is empty!" << std::endl;
		return -1;
	}*/
	icp.setInputTarget(cloud_processed);
	icp.setTransformationEpsilon(1e-10);
	icp.setMaxCorrespondenceDistance(1);
	icp.setEuclideanFitnessEpsilon(0.001);
	icp.setMaximumIterations(100);
	icp.setUseReciprocalCorrespondences(true);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp(new pcl::PointCloud<pcl::PointXYZ>);
	icp.align(*cloud_icp);

	//判断是否收敛&计算质量分数
	std::cout << "ICP has" << (icp.hasConverged() ? " converged." : " not converged.") << "score is:" << icp.getFitnessScore() << std::endl;
	Eigen::Matrix4f final_transform_icp = icp.getFinalTransformation();
	std::cout << "Final transformation: " << final_transform_icp << std::endl;

	//---------------------------------结果可视化--------------------------------
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color(cloud_in, 255, 0, 0);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_in, cloud_in_color, "cloud_in"); //初始点云
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_processed_color(cloud_processed, 0, 255, 0);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_processed, cloud_processed_color, "cloud_processed"); //采样,平移,旋转后的点云
	/*   pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_aligned_color(cloud_aligned, 0, 255, 255);
	   viewer->addPointCloud<pcl::PointXYZ>(cloud_aligned, cloud_aligned_color, "cloud_aligned");*/ //ransac粗配准后的点云
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_icp_color(cloud_icp, 0, 0, 255);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_icp, cloud_icp_color, "cloud_icp"); //icp配准后的点云
	while (!viewer->wasStopped())
	{
		viewer->spinOnce();
	}
	return (0);
}